#include <assert.h>
#include <stdlib.h>
#include <memory.h>
#include "memory_avm.h"
#include "type.h"

#include <stdio.h>

#define A20ADDR(x) (x & 0xFFFFFFFF) 

bit8u *alloc_aligned_vector(bit8u **actual_vector, bit32u bytes, bit32u alignment) {
	bit32u masked = alignment - 1;
	*actual_vector = new bit8u[(bit32u)(bytes + masked)];
	bit8u *vector = (bit8u *)(((bit32u)(*actual_vector) + masked) & ~masked);
	assert(vector >= *actual_vector);
	return vector;
}

bit8u *avm_memory::get_vector(avm_phy_addr addr) {
	return &vector[addr];
}

avm_memory::avm_memory(unsigned size) {
	actual_vector = NULL;
	vector = NULL;
	init_memory(size, 0x1000);
}

avm_memory::~avm_memory() {
	delete []actual_vector;
}

void avm_memory::init_memory(bit32u bytes, bit32u alignment) {
	if (actual_vector != NULL) {
		delete []actual_vector;
		actual_vector = NULL;
		vector = NULL;
	}

	vector = ::alloc_aligned_vector(&actual_vector, 
			bytes + BIOS_ROM_SIZE + EXTEND_ROM_SIZE + 4096, MEM_VECTOR_ALIGN);

	memsize = bytes;
	rom = &vector[memsize];
	bogus = &vector[memsize + BIOS_ROM_SIZE + EXTEND_ROM_SIZE];
#ifdef DEBUG
	printf("Memory allocation successfully.\n \
			Base address = %08X.\nAligned address = %08X.\nMemsize = %d.\n", 
			(bit32u)actual_vector, (bit32u)vector, bytes);
#endif
	memset(actual_vector, bytes, 0);
	memset(rom, BIOS_ROM_SIZE + EXTEND_ROM_SIZE + 4096, 0xFF);
}

void read_hostdword_from_little_endian(void *addr, bit32u *nativeVal32) {
	*nativeVal32 = *(bit32u *)addr;
}

void read_hostword_from_little_endian(void *addr, bit16u *nativeVal16) {
	*nativeVal16 = *(bit16u *)addr;
}

void write_hostdword_from_little_endian(void *addr, bit32u nativeVal32) {
	*(bit32u *)addr = nativeVal32;
}

void write_hostword_from_little_endian(void *addr, bit32u nativeVal16) {
	*(bit16u *)addr = nativeVal16;
}

void avm_memory::write_physical_page(v_cpu *cpu, avm_phy_addr addr, 
		bit32u len, void *p_buffer) {
	avm_phy_addr a20addr = A20ADDR(addr);
	bit8u *pdata;

	if ((a20addr >> 12) != ((a20addr + len - 1) >> 12)) {
		printf("Access across a page at address %08X, len=%d.\n", a20addr, len);
		return;
	}

	if (a20addr <= memsize) {
		if (((a20addr & 0xFFF80000) != 0x80000) || (a20addr <= 0x9FFFF)) {
			if (len == 4) {
				write_hostdword_from_little_endian(get_vector(a20addr), *(bit32u *)p_buffer);
				return;
			}
			else if (len == 2) {
				write_hostword_from_little_endian(get_vector(a20addr), *(bit16u *)p_buffer);
				return;
			}
			else if (len == 1) {
				*get_vector(a20addr) = *(bit8u *)p_buffer;
				return;
			}
			else {
				pdata = (bit8u *)p_buffer;
				while (1) {
					*get_vector(a20addr) = *pdata;
					if (len-- == 1)
						return;
					a20addr++;
					pdata++;
				}
			}
		}
		// Addr must be in range 0xA0000 - 0xFFFFF
		// Not implement yet
	}
	else {
		printf("Write out of physical at address %08X.\n", a20addr);
		return;
	}
}

/*
 * 0x80000 - 0x9FFFF Optional fixed memory hole
 * 0xA0000 - 0xBFFFF Standard VGA buffer
 * 0xC0000 - 0xDFFFF Expansion card BIOS
 * 0xE0000 - 0xEFFFF Lower BIOS
 * 0xF0000 - 0xFFFFF Upper BIOS
 * */
void avm_memory::read_physical_page(v_cpu *cpu, avm_phy_addr addr, 
		bit32u len, void *p_buffer) {
	avm_phy_addr a20addr = A20ADDR(addr);
	bit8u *pdata;

	// Access must be within a page
	if ((a20addr >> 12) != ((a20addr + len - 1) >> 12)) {
		printf("Access across a page at address 0x%08X, len = %d.\n", a20addr, len);
		return;
	}

	if (a20addr <= memsize) {
		if (((a20addr & 0xFFF80000) != 0x80000) || (a20addr <= 0x9FFFF)) {
			if (len == 4) {
				read_hostdword_from_little_endian(get_vector(a20addr), (bit32u *)p_buffer);
				return;
			}
			else if (len == 2) {
				read_hostword_from_little_endian(get_vector(a20addr), (bit16u *)p_buffer);
				return;
			}
			else if (len == 1) {
				*(bit8u *)p_buffer = *get_vector(a20addr);
				return;
			}
			else {
				// Other length
				pdata = (bit8u *)p_buffer;
				while (1) {
					*pdata = *get_vector(a20addr);
					if (len-- == 1)
						return;
					pdata++;
					a20addr++;
				}
			}
		}

		// Addr must be in range 0xA0000 - 0xFFFFF
		// Unimplement yet	
	}
	else {
		// Access out of limits of physical memory.
		pdata = (bit8u *)p_buffer;
		for (int i = 0; i < len; i++) {
			if (a20addr >= (avm_phy_addr)~BIOS_ROM_SIZE_SHIFT) {
				*pdata = rom[a20addr & BIOS_ROM_SIZE_SHIFT];
			}
			else {
				*pdata = 0xFF;
			}
			addr++;
			a20addr = addr;
			pdata++;
		}
	}
}

